%小机器最短路程跑路
%输入：起始点坐标与目的点坐标(2*2)
%输出：位移过程点(n*2)
%输出为-1则不可移动到该点
%A*的H采用Manhattan方法 系数可换
function MobileOrder=GetTrajectory(input)
global MapData;
% load('MapData.mat');
ManhattanCoefficient=1;  %A*的Manhattan系数

StarPoint=input(1,:);
EndPoint=input(2,:);
% [m,n]=size(MapData);

FlagStar=FindLineNum(StarPoint); %找到起始点的行数
FlagEnd=FindLineNum(EndPoint); %找到终点的行数

closelist(1,:)=[FlagStar,StarPoint(1,1),StarPoint(1,2),0,0,0,0,0,0,0,0,0]; %初始点加入close list
FlagOpen=1;  %open list计数  flag在加入新的行之后立刻自加
FlagClose=2; %close list计数
FlagFind=1; %A*结束判断标志
if (MapData(FlagStar,4)==0 || MapData(FlagEnd,4)==0) && FlagStar~=FlagEnd
    MobileOrder=-1;%当出发点不能移动或终点不能抵达时返回-1表示不能移动
elseif FlagStar==FlagEnd  %原地踏步走
    MobileOrder=StarPoint;
else
    %A*
    %open list & close list格式[点所在行数，X,Y，G,H,F,父点所在行数，X,Y，G,H,F]
    while FlagFind
        
        %将父点的可移动点加入open list
        %加入open list的子点需要满足三个条件
        %原若在open list中则比较G值，判断是不动，还是更新父点
        %可以去的地方需要有两个以上（除非是终点）
        %close list的点不加入open list
        for i=1:1:MapData(closelist(FlagClose-1,1),4)
            NumLine=FindLineNum([MapData(closelist(FlagClose-1,1),2*i+3),MapData(closelist(FlagClose-1,1),2*i+4)]);
            FlagRepeat=0;
            FlagInClose=0;
            for k=1:1:(FlagClose-1) %判断是否在close list中
                if NumLine==closelist(k,1)
                    FlagInClose=1;
                    break;
                end
            end
            if FlagOpen>1
                for j=1:1:FlagOpen-1 %是否在open list中
                    if NumLine==openlist(j,1)  %点已经在open list中
                        G=1+closelist(FlagClose-1,4);
                        H=ManhattanCoefficient*(abs(EndPoint(1,1)-MapData(NumLine,2))+abs(EndPoint(1,2)-MapData(NumLine,3)));
                        F=G+H;
                        if G<openlist(j,4) %若G更小则更新父点
                            openlist(j,4:6)=[G,H,F];
                            openlist(j,7:12)=closelist(FlagClose-1,1:6);
                        end
                        FlagRepeat=1;
                        break;
                    end
                end
            end
            if (MapData(NumLine,4)>1||NumLine==FlagEnd) && FlagInClose==0 && FlagRepeat==0  %不重复点
                G=1+closelist(FlagClose-1,4);
                H=ManhattanCoefficient*(abs(EndPoint(1,1)-MapData(NumLine,2))+abs(EndPoint(1,2)-MapData(NumLine,3)));
                F=G+H;
                openlist(FlagOpen,:)=[NumLine,MapData(NumLine,2),MapData(NumLine,3),G,H,F,closelist(FlagClose-1,1:6)];
                FlagOpen=FlagOpen+1;
            end
        end
        
        %open list排序，按F从大到小排列
        openlist=sortrows(openlist,-6);
        
        %若终点在open list中停止搜索
        for i=1:1:FlagOpen-1
            if openlist(i,1)==FlagEnd
                FlagFind=0;
                FatherPoint=openlist(i,7);%记录终点父点的行数
            end
        end
        
        %将F最小的移入close list
        closelist(FlagClose,:)= openlist(FlagOpen-1,:);
        FlagClose=FlagClose+1;
        FlagOpen=FlagOpen-1;
        openlist(FlagOpen,:)=[];
    end
    
    %将点序列从close list中取出
    FlagCatch=1;
    MobileOrderC(1,:)=EndPoint(1,:);
    FlagOrder=2;
    while FlagCatch
        for i=1:1:(FlagClose-1)  %找父点
            if closelist(i,1)==FatherPoint
                MobileOrderC(FlagOrder,:)=closelist(i,2:3);
                FatherPoint=closelist(i,7);
                FlagOrder=FlagOrder+1;
                break;
            end
        end
        if MobileOrderC(FlagOrder-1,:)==StarPoint(1,:) %父点为起点则完成搜索
            FlagCatch=0;
        end
    end
    
    %将点列倒叙输出结果
    for i=1:1:(FlagOrder-1)
        MobileOrder(i,:)=MobileOrderC(FlagOrder-i,:);
    end
    
end

end